﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace DroneAPI
{
    class DroneMovement
    {
        #region Takeoff/Land/Emergency/Null

        // Drone Takeoff
        public static string GetDroneTakeoff(uint sequenceNumber)
        {
            return CreateATREFCommand(sequenceNumber, ",290718208");
        }

        // Drone land
        public static string GetDroneLand(uint sequenceNumber)
        {
            return CreateATREFCommand(sequenceNumber, ",290717696");
        }

        // Drone emergency landing. Shuts off all engines, drone falls
        public static string GetDroneEmergency(uint sequenceNumber)
        {
            return CreateATREFCommand(sequenceNumber, ",290717952");
        }

        // Drone will do nothing
        public static string NullCommand(uint sequenceNumber)
        {
            return CreateATPCMDCommand(sequenceNumber, "0,0,0,0");
        }

        // returns an ATREF command
        private static string CreateATREFCommand(uint sequenceNumber, string command)
        {
            return "AT*REF=" + sequenceNumber + command + Environment.NewLine;
        }

        #endregion

        #region Movement Commands

        // Strafe drone left or right at velocity in range [-1,1]
        public static string GetDroneStrafeLeftRight(uint sequenceNumber, double velocity)
        {
            int value = FloatConversion(velocity);
            return CreateATPCMDCommand(sequenceNumber, value + ",0,0,0");
        }

        // Strafe drone forward or backward at velocity in range [-1,1]
        public static string GetDroneStrafeForwardBackward(uint sequenceNumber, double velocity)
        {
            int value = FloatConversion(velocity);
            return CreateATPCMDCommand(sequenceNumber, "0," + value + ",0,0");
        }

        // Drone ascend or descend at velocity in range [-1, 1]
        public static string GetDroneAscendDescend(uint sequenceNumber, double velocity)
        {
            int value = FloatConversion(velocity);
            return CreateATPCMDCommand(sequenceNumber, "0,0," + value + ",0");
        }

        // Rotate drone left or right at angular velocity in range [-1,1] 
        public static string GetDroneRotate(uint sequenceNumber, double velocity)
        {
            int value = FloatConversion(velocity);
            return CreateATPCMDCommand(sequenceNumber, "0,0,0," + value);
        }

        // Move the drone on all possible axes simultaneously
        public static string GetCompositeMovementCommand(uint sequenceNumber, double leftRight, double forwardBack, double ascendDescend, double rotate)
        {
            return CreateATPCMDCommand(sequenceNumber, FloatConversion((float)leftRight) + "," + FloatConversion((float)forwardBack)
                                        + "," + FloatConversion((float)ascendDescend) + "," + FloatConversion((float)rotate));
        }

        // Return a full ATPCMD command
        private static string CreateATPCMDCommand(uint sequenceNumber, string command)
        {
            return "AT*PCMD=" + sequenceNumber + ",1," + command + Environment.NewLine;
        }

        // converts a value from range [-1,1] to a signed 32-bit integer that can be used in AT*PCMD commands
        public static int FloatConversion(double velocity)
        {
            byte[] floatBytes = BitConverter.GetBytes((float) velocity);
            return BitConverter.ToInt32(floatBytes, 0);
        }

        #endregion
    }
}
